import numpy as np
import struct
import simplekml

class WGS84:
    def __init__(self):
        # 长轴
        self.r = 6378137.0
        # 地球自转速度
        self.omega_e = 7.2921151467e-5
        # WGS84椭球坐标系 第一偏心率
        self.e2 = 0.00669437999013

    def RM(self, phi):
        return self.r * (1 - self.e2) / np.power(1 - self.e2 * np.sin(phi) * np.sin(phi), 1.5)

    def RN(self, phi):
        return self.r / np.sqrt(1 - self.e2 * np.sin(phi) * np.sin(phi))

    def dN(self, lat1, lat2):
        dn = (lat1 - lat2) * self.RM(lat1)
        return dn

    def dE(self, lat1, lon1, lon2):
        de = (lon1 - lon2) * self.RN(lat1) * np.cos(lat1)
        return de

    def ECEF_to_LLA(self, x, y, z):
        """
        ECEF to Lat Lon Altitude
        :param x:
        :param y:
        :param z:
        :return:
        """
        p = np.sqrt(x ** 2 + y ** 2)
        lon = 2.0 * np.arctan2(y, x + p)
        lat = np.arctan(z / (p * (1 - self.e2)))
        h = 0
        cnt = 0
        while True:
            RN = self.RN(lat)
            h_pre = h
            h = p / np.cos(lat) - RN
            lat = np.arctan2(z, p * (1 - self.e2 * RN / (RN + h)))
            if np.fabs(h - h_pre) < 1e-8:
                break
            cnt += 1
            if (cnt > 10):
                print("error convert")
                break
        return lat, lon, h


def converGps2bin(infile, outfile):
    "转换矩阵为二进制文件，高位在前面"
    data = np.loadtxt(infile)
    with open(outfile, 'wb') as f:
        for line in data:
            for num in line:
                b = struct.pack('d', num)
                f.write(b)
    print("finished")


def convertEph2kml(infile, outfile):
    """
    fdsa dsffadfa  fdsafdsaaaasf    AFADdfasf dffasdfd
    :param infile:
    :param outfile:
    :return:
    """
    rawdata = np.loadtxt(infile, skiprows=6)
    length = rawdata.shape[0]
    prns = np.unique(rawdata[:, 2])
    kml = simplekml.Kml()
    trk = kml.newgxtrack(name="track")
    trk.stylemap.normalstyle.iconstyle.icon.href = 'http://earth.google.com/images/kml-icons/track-directional/track-0.png'
    trk.stylemap.normalstyle.linestyle.color = '99ffac59'
    trk.stylemap.normalstyle.linestyle.width = 6
    trk.stylemap.highlightstyle.iconstyle.icon.href = 'http://earth.google.com/images/kml-icons/track-directional/track-0.png'
    trk.stylemap.highlightstyle.iconstyle.scale = 1.2
    trk.stylemap.highlightstyle.linestyle.color = '99ffac59'
    trk.stylemap.highlightstyle.linestyle.width = 8
    for prn in prns:
        prn_data = rawdata[rawdata[:, 2] == prn, :]
        length = prn_data.shape[0]
        trk = kml.newgxtrack(name="%2d" % prn)
        trk.stylemap.normalstyle.iconstyle.icon.href = 'http://earth.google.com/images/kml-icons/track-directional/track-0.png'
        trk.stylemap.normalstyle.linestyle.color = 'ffffff00'
        trk.stylemap.normalstyle.linestyle.width = 6
        trk.stylemap.highlightstyle.iconstyle.icon.href = 'http://earth.google.com/images/kml-icons/track-directional/track-0.png'
        trk.stylemap.highlightstyle.iconstyle.scale = 1.2
        trk.stylemap.highlightstyle.linestyle.color = 'ffffff00'
        trk.stylemap.highlightstyle.linestyle.width = 8
        fs = 100  # 降低频率防止Google Earth崩溃
        skip_rows = 5
        wgs84 = WGS84()
        cnt = 0
        for i in range(length):

            if cnt % fs == 0:
                xyz = prn_data[i, 11:14]
                lla = wgs84.ECEF_to_LLA(xyz[0], xyz[1], xyz[2])

                trk.newgxcoord(coord=[(lla[1] * 180 / np.pi, lla[0] * 180 / np.pi, lla[2])])
                print(cnt)
                # pnt = kml.newpoint(name="%2d" % prn_data[i, 2],
                #                    coords=[(lla[1] * 180 / np.pi, lla[0] * 180 / np.pi, lla[2])])
                # pnt.style.iconstyle.icon.href = "http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png"
            cnt += 1
    kml.save(u"/media/rebeater/hd_data2/workspace/raw_data/car/20200927/M39_for_Tightly_Couple/200927111134.ephpos.kml")


if __name__ == "__main__":
    infile = "/media/rebeater/hd_data2/workspace/raw_data/car/20200927/M39_for_Tightly_Couple/200927111134.ephpos"
    outfile = infile + ".kml"
    convertEph2kml(infile, outfile)
